/**
 * moieo.net
 * @author Moieo
 */
package cn.moieo.carcontrol.payload

object PayloadUtil {
    /**
     * 构建完整数据包
     * @param payload 有效载荷数据
     * @return 完整数据包字节数组
     */
    fun buildPacket(startId: Byte, payload: ByteArray): ByteArray {
        // 1. 构造包头和长度
        val header = byteArrayOf(
            startId, (payload.size and 0xFF).toByte() // 长度转无符号byte
        )

        // 2. 合并包头和有效载荷
        val dataPart = ByteArray(header.size + payload.size)
        System.arraycopy(header, 0, dataPart, 0, header.size)
        System.arraycopy(payload, 0, dataPart, header.size, payload.size)
        val crc = dataPart.toCRC16Modbus()
        // 4. 组合完整数据包
        val fullPacket = dataPart.copyOf(dataPart.size + crc.size)
        System.arraycopy(crc, 0, fullPacket, dataPart.size, crc.size)
        return fullPacket
    }

    /**
     * 计算CRC16-Modbus校验码
     * @return CRC16校验码（小端字节序，低字节在前）
     */
    private fun ByteArray.toCRC16Modbus(): ByteArray {
        var crc = 0xFFFF
        val polynomial = 0xA001
        for (byte in this) {
            crc = crc xor (byte.toInt() and 0xFF)
            @Suppress("unused")
            for (i in 0..7) {
                crc = if (crc and 0x0001 != 0) {
                    (crc shr 1) xor polynomial
                } else {
                    crc shr 1
                }
            }
        }
        crc = crc and 0xFFFF
        return byteArrayOf(
            (crc and 0xFF).toByte(),  // 低字节
            ((crc shr 8) and 0xFF).toByte() // 高字节
        )
    }
}